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1: function main() 

2: Imax = 100; % Number of Monte Carlo Runs 

3: % T. Cehelnik 

4: 

5: t = 0:0.01:5; % Setup time samples 
6: 

7: [Dlt, Vlt, Sit] =get_it (t, 1) ; %% Get Distance, Velocity, Signal training 

8: [D2t,V2t,S2t]=get_it(t,2) ; 

9: [D3t,V3t,S3t]=get_it (t,3) ; 
10: 

11: for 1=1: Imax 
12: 

13: [Dl,Vl,Sl]=get_it(t,l) ; % get Dl 
14: 

15: [D2,V2,S2]=get_it (t,2) ; 
16: 

17: [D3,V3,S3]=get_it(t,3) ; 
18: 

19: Cll = corrcoef (Sit, SI) ; 

20: C12 = corrcoef (Sit, S2) ; 

21: C13 = corrcoef (Sit, S3) ; 
22: 

23: cll(I) = Cll(l,2) ; 

24: cl2(I) = C12(l,2) ; 

25: cl3(I) = C13(l,2) ; 
26: 

27: C21 = corrcoef (S2t, SI) ; 

28: C22 = corrcoef (S2t, S2) ; 

29: C23 = corrcoef (S2t, S3 ) ; 
30: 

31: C21(I) = C21(l,2) ; 

32: c22(I) = C22(l,2) ; 

33: c23(I) = C23(l,2) ; 
34: 

35: C31 = corrcoef (S3t, SI) ; 

36: C32 = corrcoef (S3t, S2 ) ; 

37: C33 = corrcoef (S3t, S3 ) ; 
38: 

39: c31(I) = C31(l,2) ; 

40: c32(I) = C32(l,2) ; 

41: c33(I) = C33(l,2) ; 

42 : end 
43: 

44: figure (1) 

45: plot (1: Imax, cll, ' r' , 1 : Imax, mean (cll) , 'r' ) 

46: hold 
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47: plot (1: Imax, cl2, 1: Imax, mean (cl2) ) 
48: plot (1: Imax, cl3, 'k' , 1 : Imax,mean (cl3 ) , 'k' ) 
49: 

50: figure (2) 

51: plot (1: Imax, c21, ' r \1 : Imax,mean(c21) , 'r 1 ) 
52: hold 

53: plot(l:Imax,c22,l:Imax,mean(c22) ) 

54: plot (1: Imax, c23, 'k' , 1: Imax, mean (c23) , 'k' ) 

55: 

56: figure(3) 

57: plot (1: Imax, c31, ' r 1 , 1 : Imax, mean (c31) , , r 1 ) 
58: hold 

59: plot (1: Imax, c32,l: Imax, mean (c32) ) 

60: plot (1: Imax, c33, 'k' , 1 : Imax, mean (c33) , 'k' ) 

61: 
62: 

63: figure (10) 

64: plot(t,Dl(: ,1) , ' b ' , t , Dl ( : , 2 ) , ' b . ' ) 
65: hold 

66: plot(t,Dl( : ,3) , 1 k ' , t , Dl ( : , 4) , ' k. ' ) 
67: plot(t,Dl( : ,5) , 'm' , t, Dl ( : , 6) , 'm. ' ) 
68: 

69 : return 
70: 

71: function [D, V, S] =get_it ( t , opt) 

72: %% Get vector of point used to make motional command 
73: 

74: if(opt==l) 

75: [rx,ry,rz]= sweepitx ( t ) ; 

7 6 : end 

77: 

78: if (opt==2) 

79: [rx,ry,rz]= sweepity ( t) ; 

80: end 

81: 

82: if (opt ==3) 

83: [rx,ry,rz]= sweepitz(t); 

84 : end 

85: 

86: 

87 : %% add noise 
88: 

89: variance = 1.5; 
90: 

91: rx = rx + variance. *randn(l, 1) ; 
92: ry = ry + variance. *randn(l, 1) ; 
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93: rz = rz + variance. *randn(l, 1) ; 
94: 

95: [rl,r2,r3,r4,r5,r6]= vectors (rx, ry, rz ) ; 

96: dl = dist(rl) ;d2 =dist(r2);d3 =dist(r4);d4 =dist(r4); 

97: d5 =dist(r5);d6 = dist(r6); 

98: 

99: % Get Distance D, Velocity V, and Voltage Signal S. 
100: D = [dist(rl) ,dist (r2) ,dist(r3) ,dist(r4) ,dist(r5) , . . . 
101: dist(r6)]; 

102: V= [vel(dist (rl) ) , vel (dist (r2) ) , vel (dist (r3 ) ) , . . . 
103: vel(dist(r4) ) , vel (dist (r5) ) , vel (dist (r6) ) ] ; 

104: S = [signal (dl) , signal (d2) , signal (d3) , signal (d4) ,.. . 
105: signal (d5) , signal (d6) ] ; % Simulate Signal 

106: S = S. /max(max(S) ) ; %% Normalize Signal 
107: 

108 : return 

109: 

110: 

111: function [s] = signal (d) 

112: %% Make signal proportional to 1/d; 

113: s = l./d; 

114: 

115 : return 
116: 

117: function [v] = vel(d); 

118: v = diff (d) ; 

119: 

120: 

121: return 
122: 

123: function [d]= dist(r) 
124: 

125: d = sqrt(r.x. A 2 +r.y. A 2 +r.z. A 2); 
126: d = d' ; 
127 : return 
128: 

129: function [rx, ry, rz] =sweepity (t) 

130: zo = 10; 

131: xo = 0; 

132: yo = 0 ; 

133: 

134: %rx = xo + 10 . *sin(-2*pi . *t) ; 

135: %ry = yo; 

136: 

137 : rx = xo ; 

138: ry = yo + 10 . *sin ( -2*pi . * t) ; % vert 
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rz = zo + 10 . *cos (-2*pi . *t) ; 
return 

function [rx, ry, rz] =sweepitx(t) 
zo = 10; 
xo = 0; 
yo = 0; 

rx = xo + 10.*sin(-2*pi.*t) ; % horz 
ry = yo; 

rz = zo + 10 . *cos (-2*pi . *t) ; 
return 

function [rx, ry, rz] =sweepitz (t) 
zo = 10; 
xo = 0; 
yo = 0; 

ry = yo + 10 . *sin ( -2*pi . *t) ; 
rx = xo + 10 . *cos (-2*pi . *t) ; ; 

rz = zo ; 

return 



%% vector rl = 

function [rl , r2 , r3 , r4 , r5 , r6] = vectors (rx, ry, rz) ; 



172 


: xl 




20; 


173 


: yi 




20; 


174 


: z5 




5; 




175 










176 


: rl 


.X 




rx - xl; 


177 


: r2 


.X 




rx + xl; 


178 


: r3 


.X 




rx; 


179 


: r4 


.X 




rx; 


180 


: r5 


.X 




rx; 


181 


: r6 


.X 




rx; 


182 










183 










184 


: rl 


• y 




ry; 
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1 QC , 

loo : 


rz . y 




ry ; 


1 o c . 

lob : 


r3 .y 






lo / : 


r4 . y 




ry+yi ; 


1 o o • 

loo : 


r5 . y 




ry-yi ; 


1 oo • 

loy : 


r6 . y 




ry+yl ; 


ion. 

iy U : 








1 O 1 . 

iy 1 : 


n . z 




rz ; 


1 oo . 


. Z 




rz ; 


1 y j : 


IJ . z 




rz ; 


194 : 


r4 . z 




rz ; 


195 : 


r5 . z 




rz-z5 ; 


196: 


r6 . z 




rz +z5; 


197: 








198: 


return 




199: 









